#include <iostream>
#include <fstream>
#include <vector>
#include <pthread.h>
#include <unistd.h>
#include <string.h>

#include "driver_Xbox_Joystick.h"
#include "project.h"
#include "driver_usart.h"
#include "com_parse_data.h"

typedef struct
{
    pthread_t joystic_thread;
    pthread_t serial_thread;
    pthread_t serial_thread_sand;
} APP_CTX;

std::vector<uint8_t> m_usratBuf;
APP_CTX app_ctx = {0};

#define DATA_PARASE_HEAD   ('F')
#define DATA_PARASE_END    (0x0A)

int32_t line_speed = 0;
int32_t angle_speed = 0;


void *monitorJoyStick(void *ptr)
{
    JoyStickDriver JoyStick; // 手柄控制驱动对象
    int len;
    xbox_map_t joystickMap = {0};
    int32 recv = 0;

    recv = JoyStick.JoyStickInit();
    if (recv != REV_OK) {
        GUA_LOGE("init joystic error!\n");
        return 0;
    }

    while(1) {
        len = JoyStick.JoyStickRead(&joystickMap);
        if (len < 0) {
            usleep(20*1000);
            continue;
        }
        usleep(10*1000);
        // GUA_LOGI("\rTime:%8d A:%d B:%d X:%d Y:%d LB:%d RB:%d select:%d start:%d lo:%d ro:%d XX:%-6d YY:%-6d LX:%-6d LY:%-6d RX:%-6d RY:%-6d LT:%-6d RT:%-6d",
        //         joystickMap.time, joystickMap.a, joystickMap.b, joystickMap.x, joystickMap.y, joystickMap.lb, joystickMap.rb, joystickMap.select, joystickMap.start, joystickMap.lo, joystickMap.ro,
        //         joystickMap.xx, joystickMap.yy, joystickMap.lx, joystickMap.ly, joystickMap.rx, joystickMap.ry, joystickMap.lt, joystickMap.rt);

        line_speed = (int) joystickMap.ly;
        angle_speed = (int)joystickMap.lt;
        GUA_LOGI("line_speed %d  angle_speed %d\n", line_speed, angle_speed);
    }

    JoyStick.JoyStickClose();
    return 0;
}


void int32_to_int8_array(int32_t value, uint8_t* out) {
    // 大端：高字节在前
    out[0] = (value >> 24) & 0xFF;
    out[1] = (value >> 16) & 0xFF;
    out[2] = (value >> 8) & 0xFF;
    out[3] = value & 0xFF;
}


uint8 chan_data_sum(uint8_t *load_buf, uint32 data_len)
{
    uint32 i = 0;
    uint8 sum_result = 0;

    for (i = 0; i < data_len; i++) {
        sum_result += load_buf[i];
        // GUA_LOGI("[%d]=[%x] [%d]\n", i, load_buf[i], sum_result);
    }
    return sum_result;
}


void *serial_control_sand(void *ptr)
{
    SerialDriver * serial = (SerialDriver *)ptr;
    uint8_t data_buf[16] = {0};
    uint8_t dat[4] = {0};
    int data_sun = 0;

    data_buf[0] = DATA_PARASE_HEAD;
    data_buf[1] = 8;
    data_buf[15] = DATA_PARASE_END;

    while (1) {
        usleep(1000*50); // 20Hz

        int32_to_int8_array(line_speed, dat);
        for (size_t i = 0; i < 4; i++) {
            data_buf[i+2] = dat[i];
        }

        int32_to_int8_array(angle_speed, dat);
        for (size_t i = 0; i < 4; i++) {
            data_buf[i+6] = dat[i];
        };

        data_buf[14] = chan_data_sum(data_buf+2, 8);
        // GUA_LOGI("data_buf[14]=%d\n", data_buf[14]);
        serial->Write(data_buf, 16);
    }

}


void *serial_control(void *ptr)
{
    SerialDriver chassis_serial("/dev/serial0", BPS921600); // 串口驱动对象，包含了对串口的初始化
    int32 rev = 0;
    PARASE_BUFFER_STRUCT data_structure = {0};

    data_structure.head = 'F';
    data_structure.check_type = SUM_CHEACK;
    data_structure.end = 0x0A;
    uint8_t m_usratBuf[256] = {0};

    rev = pthread_create(&app_ctx.serial_thread_sand, NULL, serial_control_sand, &chassis_serial);
    if(rev) {
        GUA_LOGE("Create pthread error!");
    }

    while(1) {
        rev = getUsartData(m_usratBuf, &chassis_serial, &data_structure);
        if (rev != RECV_DONE) {
            continue;
        }

        rev = com_parse_data(m_usratBuf);
        if (rev != REV_OK) {
            continue;
        }
    }
}

int main(int argc, char *argv[])
{

    int32 rev = 0;

    GUA_LOGI("class begin\n");

    rev = pthread_create(&app_ctx.joystic_thread, NULL, monitorJoyStick, NULL);
    if(rev) {
        GUA_LOGE("Create pthread error!");
        return 1;
    }

    rev = pthread_create(&app_ctx.serial_thread, NULL, serial_control, NULL);
    if(rev) {
        GUA_LOGE("Create pthread error!");
        return 1;
    }

    while (1) {
        sleep(1000);
    }
}


